package com.suray.rgv60.core;

import com.alibaba.fastjson.JSONObject;
import com.suray.basic.wcs.utils.LoggerType;
import com.suray.basic.wcs.utils.Time;
import com.suray.commin.core.utils.StringUtils;
import com.suray.rgv.util.ErrorInfo;
import com.suray.rgv.util.RgvLogger;
import com.suray.rgv60.util.Rgv60CoreConfigParam;
import com.suray.rgv60.util.Rgv60ErrorUtil;
import com.suray.rgv60.util.RobotIOType;
import com.suray.type.*;
import suray.rgv.communication.data.status.RobotInfoData1;

import java.util.*;

/**
 * 两栖车数据
 */
public class Rgv60Info {
    //中断
    protected boolean interrupt;

    //当前站点
    protected String currentStation;

    //定位状态
    protected SeerRobotPositionStatus relocStatus;

    //机器人当前实时站点
    protected String robotCurrentStation;

    //上一个点的站点
    protected String lastStation;

    //序列号
    protected String serialNum;

    //在线
    protected boolean onLine;

    //有小车控制权
    protected boolean hasRgvAuthor;

    //小车任务状态
    protected Rgv60Status rgvStatus;

    //小车实际状态
    protected Rgv60Status rgvActualStatus;

    //故障
    protected Set<ErrorInfo> errorInfos;

    //托盘状态
    protected Rgv60PalletStatus palletStatus;

    //托盘是否启用
    protected Boolean palletEnable;

    //托盘动作异常码
    protected Integer palletActionErrorCode;

    //小车是否载货
    protected Boolean hasCargo;

    //x坐标,距离原点的距离
    protected Double x;

    //y坐标,距离原点的距离
    protected Double y;

    //angle坐标
    protected Float angle;

    //位置可信度0-1
    protected Double confidence;

    //障碍物阻挡
    protected Boolean blocked;

    //阻挡原因
    protected Rgv60BlockReason blockReason;

    //电池电量0-1
    protected Float batSoc;

    //电池温度,℃%
    protected Float batTemp;

    //电池电压
    protected Float batVolt;

    //电池电流
    protected Float batCur;

    //是否连接充电桩
    protected Boolean autoCharge;

    //充电中
    protected Boolean charging;

    //任务状态, 导航状态
    protected Rgv60TaskStatus taskStatus;

    //当前任务的终点，目的位置
    protected String targetStation;

    //任务类型
    protected Rgv60TaskType taskType;

    //加载地图状态
    protected Rgv60LoadMapStatus loadMapStatus;

    //运行模式
    protected Rgv60Mode mode;

    //运行模式
    protected Rgv60RollerMode rollerMode;

    //当前地图名字
    protected String currentMap;

    //当前地图MD5
    protected String currentMapMD5;

    //当前连接 Wifi 的信号强度，0-100 百分比（机器人需要支持连接 Wifi 并且已经连接，否则为0）
    protected Integer wifiStrength;

    //机器人名字
    protected String robotName;

    //小车存储的地图
    protected Set<String> maps;

    //累计里程,km
    protected Double odo;

    //今日累计里程,m
    protected Double todayOdo;

    //累计运行时间
    protected Time runningTime;

    //本次运行时间
    protected Time curRunningTime;

    //控制器电压
    protected Float controlVolt;

    //控制器温度
    protected Float controlTemp;

    //控制器湿度
    protected Float controlWet;

    //电池循环次数
    protected int chargeNum;

    //支持最大充电电压
    protected Float permitMaxChgVolt;

    //支持最大充电电流
    protected Float permitMaxChgCur;

    //障碍物x坐标
    protected Float blockX;

    //障碍物y坐标
    protected Float blockY;

    //急停
    protected Boolean driveEmc;

    //抱闸
    protected Boolean holdBrake;

    //底盘是否静止
    protected Boolean isStop;

    //目标点x，y，角度
    protected Float targetX;
    protected Float targetY;
    protected Float targetR;

    //机器人备注
    protected String robotNote;

    //货叉高度
    protected Float forkHeight;

    //货叉到位
    protected Boolean forkArrived;

    //货叉控制模式
    protected Boolean forkAutoMode;

    /**
     * DI，DO光电状态
     */
    protected Map<RobotIOType.ROBOT_DI_TYPE, Boolean> DIs;
    protected Map<RobotIOType.ROBOT_DO_TYPE, Boolean> DOs;


    public Rgv60Info(String serialNum) {
        this.serialNum = serialNum;
        this.palletEnable = false;
        this.relocStatus = SeerRobotPositionStatus.INVALID;
        this.palletStatus = Rgv60PalletStatus.INVALID;
        this.interrupt = false;
        this.batSoc = 0f;
        this.batTemp = 0f;
        this.hasRgvAuthor = false;
        this.taskStatus = Rgv60TaskStatus.INVALID;
        this.taskType = Rgv60TaskType.INVALID;
        this.loadMapStatus = Rgv60LoadMapStatus.INVALID;
        this.mode = Rgv60Mode.STAND_ALONE;
        this.hasCargo=false;
        this.blocked=false;
        this.autoCharge=false;
        this.charging=false;
        this.runningTime=new Time(0);
        this.curRunningTime=new Time(0);
        this.driveEmc=false;
        this.holdBrake=false;
        this.isStop=false;
        this.rollerMode = Rgv60RollerMode.INVALID;
        this.onLine = false;
        this.hasRgvAuthor = false;
        this.rgvActualStatus = Rgv60Status.NONE;

        this.errorInfos = new HashSet<>();
        this.maps = new HashSet<>();
        this.DIs = new HashMap<>();
        this.DOs = new HashMap<>();
    }

    public String getSerialNum() {
        return serialNum;
    }

    /**
     * 添加故障
     * @param errorInfo
     */
    protected void addError(ErrorInfo errorInfo) {
        if (errorInfo == null) {
            return;
        }

        for (ErrorInfo error : errorInfos) {
            if (error.getErrorCode() == errorInfo.getErrorCode()) {
                if (error.updateError(errorInfo.getErrorLevel(), errorInfo.getErrorMessage())) {
                    RgvLogger.info(LoggerType.INFO, errorInfo.getErrorCode(), serialNum, "升级故障等级：" + errorInfo,
                            this.getClass());
                }
                return;
            }
        }

        RgvLogger.info(LoggerType.INFO, errorInfo.getErrorCode(), serialNum, "添加故障：" + errorInfo,
                this.getClass());
        errorInfos.add(errorInfo);
    }

    /**
     * 添加故障
     * @param errorUtil
     */
    protected void addError(Rgv60ErrorUtil errorUtil) {
        ErrorInfo errorInfo = new ErrorInfo(errorUtil.getCode(), errorUtil.getDes(), errorUtil.getLevel());
        for (ErrorInfo error : errorInfos) {
            if (error.getErrorCode() == errorInfo.getErrorCode()) {
                if (error.updateError(errorInfo.getErrorLevel(), errorInfo.getErrorMessage())) {
                    RgvLogger.info(LoggerType.INFO, errorInfo.getErrorCode(), serialNum, "升级故障等级：" + errorInfo,
                            this.getClass());
                }
                return;
            }
        }

        RgvLogger.info(LoggerType.INFO, errorInfo.getErrorCode(), serialNum, "添加故障：" + errorInfo,
                this.getClass());
        errorInfos.add(errorInfo);
    }

    /**
     * 清除故障
     */
    protected void clearError() {
        errorInfos.clear();
    }


    /**
     * 删除故障
     * @param errorInfo
     */
    protected void deleteErrorInfo(ErrorInfo errorInfo) {
        errorInfos.remove(errorInfo);
    }

    /**
     * 删除故障
     * @param errorCode
     */
    protected void deleteErrorInfo(int errorCode) {
        for (ErrorInfo error : errorInfos) {
            if (error.getErrorCode() == errorCode) {
                errorInfos.remove(error);
                return;
            }
        }
    }

    /**
     * 获取最严重的故障
     * @return
     */
    protected ErrorInfo getHighestError() {
        ErrorInfo error = null;
        for (ErrorInfo errorInfo : errorInfos) {
            if (error == null) {
                error = errorInfo;
            } else if (errorInfo.compareLevel(error.getErrorLevel())) {
                error = errorInfo;
            }
        }

        return error;
    }

    public boolean checkErrorLevel(ErrorInfo errorInfo) {
        return errorInfo.compareLevel(ErrorLevel.ERROR_LEVEL_4);
    }

    protected void setInterrupt() {
        this.interrupt = true;
    }

    protected boolean isInterrupt() {
        return interrupt;
    }

    protected void clearInterrupt() {
        this.interrupt = false;
    }

    protected void initRgvInfo() {
        this.interrupt = false;
        this.errorInfos.clear();
    }

    protected void analyzeRgv60Info(JSONObject jsonObject) {
        RgvLogger.debug(LoggerType.STATUS_INFO, serialNum, "获取车辆状态", jsonObject.toJSONString(), this.getClass());
        RobotInfoData1 robotInfoData1 = JSONObject.parseObject(jsonObject.toJSONString(), RobotInfoData1.class);

        relocStatus = SeerRobotPositionStatus.parseStatus(robotInfoData1.getRelocStatus());
        robotCurrentStation = robotInfoData1.getCurrentStation();
        if (relocStatus == SeerRobotPositionStatus.SUCCESS) {
            if (StringUtils.isNotEmpty(robotCurrentStation)) {
                this.currentStation = robotCurrentStation;
            }
        } else if (rgvStatus == Rgv60Status.STANDY) {
            //定位不准确，需人工确认定位
            this.currentStation = null;
        }

        lastStation = robotInfoData1.getLastStation();
        confidence = robotInfoData1.getConfidence();
        palletActionErrorCode = robotInfoData1.getJackErrorCode();
        x = robotInfoData1.getX();
        y = robotInfoData1.getY();
        angle = robotInfoData1.getAngle().floatValue() / Rgv60CoreConfigParam.PI * 180;
        blocked = robotInfoData1.getBlocked();
        blockReason = Rgv60BlockReason.parseReason(robotInfoData1.getBlockReason());
        batSoc = robotInfoData1.getBatteryLevel();
        batTemp = robotInfoData1.getBatteryTemp();
        batVolt = robotInfoData1.getVoltage();
        batCur = robotInfoData1.getCurrent();
        autoCharge = robotInfoData1.getAutoCharge();
        charging = robotInfoData1.getCharging();
        taskStatus = Rgv60TaskStatus.parseStatus(robotInfoData1.getTaskStatus());
        targetStation = robotInfoData1.getTargetId();
        taskType = Rgv60TaskType.parseValue(robotInfoData1.getTaskType());
        loadMapStatus = Rgv60LoadMapStatus.parseStatus(robotInfoData1.getLoadmapStatus());
        palletStatus = Rgv60PalletStatus.parseStatus(robotInfoData1.getJackState());
        palletEnable = robotInfoData1.getJackEnable();
        hasCargo = robotInfoData1.getJackIsfull();
        mode = Rgv60Mode.parseMode(robotInfoData1.getDispatchMode());
        rollerMode = Rgv60RollerMode.parseMode(robotInfoData1.getRollerMode());
        odo = robotInfoData1.getOdo() / 1000;
        todayOdo = robotInfoData1.getTodayOdo();
        runningTime = new Time(robotInfoData1.getTotalTime());
        curRunningTime = new Time(robotInfoData1.getTime());
        controlVolt = robotInfoData1.getControllerVoltage();
        controlTemp = robotInfoData1.getControllerTemp();
        controlWet = robotInfoData1.getControllerHumi();
        chargeNum = robotInfoData1.getBatteryCycle();
        permitMaxChgVolt = robotInfoData1.getMaxChargeVoltage();
        permitMaxChgCur = robotInfoData1.getMaxChargeCurrent();
        blockX = robotInfoData1.getBlockX();
        blockY = robotInfoData1.getBlockY();
        driveEmc = robotInfoData1.getDriverEmc();
        holdBrake = robotInfoData1.getEmergency();
        isStop = robotInfoData1.getIsStop();
        if (robotInfoData1.getTargetPoint() != null && robotInfoData1.getTargetPoint().size() > 0) {
            targetX = robotInfoData1.getTargetPoint().get(0).floatValue();
            targetY = robotInfoData1.getTargetPoint().get(1).floatValue();
            targetR = robotInfoData1.getTargetPoint().get(2).floatValue() / Rgv60CoreConfigParam.PI * 180;
        }

        try {
            analyzeErrors(jsonObject);
        } catch (Exception e) {
            RgvLogger.exceptionLog(LoggerType.WARN, null, serialNum, e, this.getClass());
        }

        currentMap = robotInfoData1.getCurrentMap();
        currentMapMD5 = robotInfoData1.getCurrentMapMd5();
        wifiStrength = robotInfoData1.getRssi();
        robotName = robotInfoData1.getVehicleId();
        robotNote = robotInfoData1.getRobotNote();

        forkHeight = robotInfoData1.getForkHeight();
        forkArrived = robotInfoData1.getForkHeightInPlace();
        forkAutoMode = robotInfoData1.getForkAutoFlag();

        hasRgvAuthor = checkIfHasAuthor(robotInfoData1.getCurrentLock());

        analyzeRgvStatus(robotInfoData1);

        readRobotIO(robotInfoData1);
    }

    /**
     * 检测是否拥有控制权
     * @param lockInfo
     * @return
     */
    private boolean checkIfHasAuthor(RobotInfoData1.CurrentLockDTO lockInfo) {
//        try {
//            List<String> ips = WcsFuncUtil.getIpAddress();
//            if (!ips.contains(lockInfo.getIp())) {
//                return false;
//            }
//        } catch (SocketException e) {
//            RgvLogger.info(LoggerType.WARN, serialNum, "获取本机Ip失败", "无法100%确定是否拥有控制权", this.getClass());
//        }
//
//        if (!lockInfo.getPort().equals(ConfigurationParam.PORT)) {
//            return false;
//        }

        String nickName = lockInfo.getNickName();
        if (StringUtils.isEmpty(nickName)) {
            return false;
        }
        return nickName.equals(serialNum);
    }

    public boolean isHasRgvAuthor() {
        return hasRgvAuthor;
    }

    /**
     * 解析故障
     * @param jsonObject
     */
    private void analyzeErrors(JSONObject jsonObject) {
        analyzeErrors(jsonObject, "fatals", ErrorLevel.ERROR_LEVEL_2);
        analyzeErrors(jsonObject, "errors", ErrorLevel.ERROR_LEVEL_2);
        analyzeErrors(jsonObject, "warnings", ErrorLevel.ERROR_LEVEL_4);
        analyzeErrors(jsonObject, "notices", ErrorLevel.ERROR_LEVEL_5);

        if (taskStatus == Rgv60TaskStatus.FAILED) {
            addError(Rgv60ErrorUtil.ROUTE_ERROR);
        }
    }

    private void analyzeErrors(JSONObject jsonObject, String errorType, ErrorLevel errorLevel) {
        for (Object obj : jsonObject.getJSONArray(errorType)) {
            JSONObject jsonObj = (JSONObject) obj;
            for (String key : jsonObj.keySet()) {
                if (!key.equals("desc") && !key.equals("times")) {
                    int code = Integer.valueOf(key);
                    String message = jsonObj.getString("desc");
                    addError(new ErrorInfo(code, message, errorLevel));
                    break;
                }
            }
        }
    }

    /**
     * 解析小车状态
     * @param robotInfoData1
     */
    private void analyzeRgvStatus(RobotInfoData1 robotInfoData1) {
        switch (taskStatus) {
            case RUNNING:
                setSafeRgvStatus(Rgv60Status.RUNNING);
                rgvActualStatus = Rgv60Status.RUNNING;
                break;
            case INVALID:
                setSafeRgvStatus(Rgv60Status.NONE);
                rgvActualStatus = Rgv60Status.NONE;
                break;
            case NONE:
            case COMPLETED:
            case CANCELED:
                if (hasRgvAuthor) {
                    setSafeRgvStatus(Rgv60Status.STANDY);
                    rgvActualStatus = Rgv60Status.STANDY;
                } else {
                    setSafeRgvStatus(Rgv60Status.INIT);
                    rgvActualStatus = Rgv60Status.INIT;
                }
                break;
            case SUSPENDED:
                setSafeRgvStatus(Rgv60Status.STOP);
                rgvActualStatus = Rgv60Status.STOP;
                break;
            case FAILED:
                if (hasRgvAuthor) {
                    setSafeRgvStatus(Rgv60Status.STANDY);
                } else {
                    setSafeRgvStatus(Rgv60Status.INIT);
                }
                rgvActualStatus = Rgv60Status.ERROR;
                break;
            default:
                RgvLogger.warn(LoggerType.INFO, taskStatus.getValue(), serialNum, "小车任务状态异常",
                        this.getClass());
                break;
        }

        if (errorInfos.size() > 0 && checkErrorLevel(getHighestError())) {
            //任务失败且最严重故障高于4级时为故障状态
            rgvActualStatus = Rgv60Status.ERROR;
        }
    }

    protected void setRgvStatus(Rgv60Status rgvStatus) {
        this.rgvStatus = rgvStatus;
    }

    protected void setSafeRgvStatus(Rgv60Status rgvStatus) {
        if (this.rgvStatus != Rgv60Status.ERROR) {
            this.rgvStatus = rgvStatus;
        }
    }

    public List<ErrorInfo> getErrors() {
        return new ArrayList<>(errorInfos);
    }

    /**
     * 读取IO信号
     * @param robotInfoData1
     */
    public void readRobotIO(RobotInfoData1 robotInfoData1) {
        if (robotInfoData1.getDI() != null) {
            for (RobotInfoData1.DIDTO didto : robotInfoData1.getDI()) {
                RobotIOType.ROBOT_DI_TYPE diType = RobotIOType.ROBOT_DI_TYPE.parseValue(didto.getId());
                if (diType != RobotIOType.ROBOT_DI_TYPE.INVALID) {
                    DIs.put(diType, didto.getStatus());
                }
            }
        }

        if (robotInfoData1.getDO() != null) {
            for (RobotInfoData1.DODTO dodto : robotInfoData1.getDO()) {
                RobotIOType.ROBOT_DO_TYPE doType = RobotIOType.ROBOT_DO_TYPE.parseValue(dodto.getId());
                if (doType != RobotIOType.ROBOT_DO_TYPE.INVALID) {
                    DOs.put(doType, dodto.getStatus());
                }
            }
        }
    }

    public Rgv60TaskStatus getTaskStatus() {
        return taskStatus;
    }

    public Rgv60Status getRgvStatus() {
        return rgvStatus;
    }

    public boolean isOnLine() {
        return onLine;
    }

    public Map<RobotIOType.ROBOT_DI_TYPE, Boolean> getDIs() {
        return DIs;
    }

    public Map<RobotIOType.ROBOT_DO_TYPE, Boolean> getDOs() {
        return DOs;
    }

    public Float getForkHeight() {
        return forkHeight;
    }

    public Integer getForkHeightMM() {
        if (forkHeight == null) {
            return null;
        }

        return (int) (forkHeight * 1000);
    }

    public Boolean getForkArrived() {
        return forkArrived;
    }

    public Boolean getForkAutoMode() {
        return forkAutoMode;
    }

    public String getCurrentStation() {
        return currentStation;
    }
}
